# -*- coding: utf-8 -*-
"""
mathmodeling_SAK.Calc3D.clac_3D
===============================

定义了建模过程中三维坐标运算的一些基本的方法。

"""

import numpy as np

from scipy.optimize import minimize

from ._data_process import scale_points_and_distances
from ._data_process import unscale_point_and_distance

def average_euclidean_distance(croods):
    """
    计算给定空间坐标点集之间的平均欧氏距离。

    参数
    ----------
    croods ：np.array
        一个二维数组，每行代表一个点的坐标。

    返回值
    ----------
    float
        点集之间的平均欧氏距离。
    """
    # 计算每对点之间的差异
    diff = croods[:, np.newaxis, :] - croods[
        np.newaxis, :, :]
    # 计算每对点之间的欧氏距离
    distances = np.sqrt(np.sum(diff**2, axis=-1))
    # 去除自身与自身的比较产生的零距离
    distances = distances[np.nonzero(distances)]
    # 计算平均距离
    average_distance = np.mean(distances)
    return average_distance

def euclidean_distances_list(pos, croods):
    """
    计算给定一点 :math:`p` 和一组坐标之间的欧氏距离，并以列表的形式返
    回点 :math:`p` 到各个点之间的距离。

    参数
    ----------
    pos : array like
        给定的坐标点 :math:`p`
    croods : array like
        一组坐标之间的坐标，每行代表一个坐标点

    返回值
    ----------
    list
        点 :math:`p` 到各个点之间的距离
    """
    pos = np.array(pos)
    croods = np.array(croods)
    
    distances = []
    for crood in croods:
        distance = np.sqrt(
            np.sum(
                (crood - pos)**2
                )
            )
        distances.append(distance)
        
    return distances

# 计算距离函数
def get_croods_dis_mat(coords):
    """ 
    计算一组 N 维空间的坐标点之间的相对距离，并以 OD 矩阵的形式返回计
    算的结果。

    输入参数
    ----------
    coords : numpy.array
        _description_

    返回值
    ----------
    numpy.array
        空间坐标点之间距离的 OD 矩阵
    """
    num_points = len(coords)
    distances = np.zeros((num_points, num_points))
    for i in range(num_points):
        for j in range(i + 1, num_points):
            distances[i, j] = np.linalg.norm(
                coords[i] - coords[j])
            distances[j, i] = distances[i, j]
    return distances

def locate_target(
        points, distances, 
        print_result=False,
        optimize_kwargs={}):
    r"""
    通过迭代算法计算四点定位，确定一个空间中的目标点位置。其中，空间可
    以是任意的维度数。
    
    方法会自动对数据进行标准化，无需额外的数据标准化运算即可直接调用。
    依赖工具包 `scipy.optimize` 。如果优化失败，方法会自动返回优化
    失败结果的 `message` 。
    
    数学原理
    ------------
    
    Minimize 函数的优化目标函数实际上是一个最小化目标，它尝试找到一个
    点 :math:`\mathbf{x} = (x, y, z)` 使得该点到一组给定点之间的
    距离与已知距离之差的平方和最小。这是一个优化问题，其中优化的目标函数
    计算了所有给定点到目标点的预期距离与实际距离之间的误差平方和。

    目标函数的数学表达式：

    给定点集 :math:`\{p_i\}` 和每个点到目标点的距离 :math:`d_i`，
    目标点的坐标为 :math:`\mathbf{x}`。则目标函数 
    :math:`f(\mathbf{x})` 可以表达为：
    
    .. math::
        
        f(\mathbf{x}) = \sum_{i=1}^n {
            \left( \sqrt{
                \sum_{j=1}^3 (x_j - p_{ij})^2} - d_i
                \right)^2}

    其中：
    
    - :math:`n` 点的数量
    - :math:`x_j` 目标点 :math:`\mathbf{x}` 的第 
    :math:`j` 个坐标
    - :math:`p_{ij}` 第 :math:`i` 个点的第 :math:`j` 
    个坐标
    - :math:`d_i` 从第 :math:`i` 个点到目标点的已知距离

    在上述的目标函数中，最外层求和符号内所包裹的每一项，分别表示目标点 
    :math:`\mathbf{x}` 与第 :math:`i` 个点之间的欧几里得距离与已
    知距离 :math:`d_i` 的差值的平方。这种形式确保了误差是非负的，并
    且当实际距离与已知距离完全相同时，误差为零。求和运算将所有点的误差
    累加起来，目标是找到一个点 :math:`\mathbf{x}` ，使这个累加的误差
    尽可能小。

    最小化这个函数通常需要用到数值方法，因为含有平方根和平方，所形成的
    是一个非线性最小化问题。

    参数
    ------------
    
    points : array_like
        四个点的坐标列表，形式为
        
        >>> [[x1, y1, z1], 
             [x2, y2, z2], 
             [x3, y3, z3], 
             . . .       ]
             
    distances : array_like
        目标点到这四个点的距离列表，形式为 `[d1, d2, d3, d4]`
        
    print_result : bool
        是否打印 `scipy.optmize.minimal` 返回的 result,
        default by False.
        
    optimize_kwargs : dict
        `scipy.optmize.minimal` 的优化选项

    返回值
    ------------
    
    numpy.array
        目标点的坐标，形式为 `[x, y, z]`
    """

    # 标准化数据：
    (
        scaled_points, 
        scaled_distances, 
        min_vals, 
        scale
        ) = scale_points_and_distances(
            points, distances)

    # 定义最小化的目标函数
    def objective(x):
        return sum(
            (np.sqrt(
                np.sum((x - np.array(point)) ** 2)
                ) - distance) ** 2
                   for point, distance in zip(
                       scaled_points, scaled_distances))
    
    # 初始猜测（使用点坐标的平均值）
    initial_guess = np.mean(
        scaled_points, axis=0).flatten()
    
    # 调用 minimize 函数进行优化
    result = minimize(
        objective, initial_guess, 
        **optimize_kwargs)
    
    if print_result:
        print(result)
    
    if result.success:
        return unscale_point_and_distance(
                result.x, min_vals, scale), result
    else:
        print(
            "Optimization failed: " + result.message
        )
        return unscale_point_and_distance(
                result.x, min_vals, scale), result